#!/usr/bin/env python3
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
from launch_ros.substitutions import FindPackageShare
import yaml
import os
import json
import logging
from datetime import datetime
from launch.actions import ExecuteProcess

logger = logging.getLogger("terminal_launch_logger")

def GetConfig():
    """读取并解析USV配置文件"""
    current_path = os.path.abspath('.')
    yaml_path = os.path.join(current_path, "ControlNode/usv_control.yaml")
    
    if not os.path.exists(yaml_path):
        logger.error(f"配置文件未找到: {yaml_path}")
        raise FileNotFoundError(f"配置文件未找到: {yaml_path}")
    
    try:
        with open(yaml_path, 'r', encoding='utf-8') as file:
            yaml_content = ''.join(line for line in file if not line.strip().startswith('#'))
            return yaml.safe_load(yaml_content)
    except Exception as e:
        logger.error(f"配置文件解析失败: {str(e)}")
        raise

def generate_launch_description():
    launchDesc = LaunchDescription()
    config_data = GetConfig()

    # 生成时间戳用于文件名
    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
    
    # 自动开始录制所有话题
    bag_record = ExecuteProcess(
        cmd=['ros2', 'bag', 'record', '-a', '--storage', 'mcap', '-o', f'usv_session_{timestamp}'],
        output='screen',
        name='rosbag2_recorder',
        shell=True  # 确保在后台运行
    )
    
    # 添加录制动作
    launchDesc.add_action(bag_record)
    print(f"开始录制所有话题数据到: usv_session_{timestamp}")
    
    # 验证基本结构
    if 'types' not in config_data:
        print("错误: 配置文件中缺少'ships'根节点")
        return []
    
    # 遍历所有舰船类型
    for type_idx, type_config in enumerate(config_data['types']):
        type_id = type_config.get('type', type_idx)
        ships = type_config.get('ships', [])
        # ship_count += len(ships)
        
        print(f"类型 {type_id}: {len(ships)} 艘船")
        
        # 为每艘船创建节点
        for ship_idx, ship in enumerate(ships):
            ship_id = ship.get('id', ship_idx)
            
            # 只提取该艘船的配置
            ship_config = {
                'type': type_id,
                'id': ship_id,
                'waypoint_count': len(ship.get('waypoints', [])),
                'waypoints': ship.get('waypoints', [])
            }
            
            # 生成唯一的节点名称
            node_name = f"plan_{type_id}_{ship_id}"
            
            # 将配置转换为JSON字符串
            ship_config_json = json.dumps(ship_config)
            
            # 创建节点
            ship_node = Node(
                package='xt_user',
                executable='xt_user_node',
                name=node_name,
                parameters=[
                    {'ship_config': ship_config_json}
                ],
                output='screen'
            )
            launchDesc.add_action(ship_node)
            # launchDesc.append(ship_node)
            
            print(f"  🚢 船 {ship_id}: 配置长度 {len(ship_config_json)} 字节")
    
    # print(f"共计创建 {ship_count} 个船节点")
    
    return launchDesc